#include "frame_drive.h"
#include <stdexcept>
FrameDrive::FrameDrive(QObject *parent) : Frame(10, 0x000030, parent) {}

QByteArray FrameDrive::EncodeToBytes() const
{
    QByteArray array = QByteArray(Size(), 0);
    unsigned short code16;
    // 1
    array[0] = static_cast<unsigned char>(gas_pedal_depth * 100);
    // 2
    code16 = static_cast<unsigned short>(steering_wheel_corners * 600);
    array[1] = code16 & 0xff;
    array[2] = code16 >> 8;
    // 2
    array[3] = static_cast<unsigned char>(brake_pedal_depth * 100);
    // 3
    array[4] &= 0b0001'1111;
    array[4] |= static_cast<unsigned char>(gear_selection) << 5;
    // 4
    array[4] &= 0b1110'0111;
    array[4] |= static_cast<unsigned char>(steering_mode) << 3;
    // 5
    array[4] &= 0b1111'1011;
    array[4] |= static_cast<unsigned char>(power_off) << 2;
    // 6
    array[4] &= 0b1111'1101;
    array[4] |= static_cast<unsigned char>(emergency_stop) << 1;
    // 7
    array[4] &= 0b1111'1110;
    array[4] |= static_cast<unsigned char>(parking) << 0;
    // 8
    array[5] = static_cast<unsigned char>(speed_limit);
    // 9
    array[6] &= 0b0011'1111;
    array[6] |= static_cast<unsigned char>(control_mode) << 6;
    // 10
    array[6] &= 0b1111'0011;
    array[6] |= static_cast<unsigned char>(hardcore_mode) << 2;
    // 11
    array[6] &= 0b1111'1101;
    array[6] |= static_cast<unsigned char>(D1_temporary_switch) << 1;
    // 12
    array[6] &= 0b1111'1110;
    array[6] |= static_cast<unsigned char>(dodge_mode) << 0;
    // 13
    array[7] &= 0b1110'1111;
    array[7] |= static_cast<unsigned char>(low_pressure_pump_control) << 4;
    // 14
    array[7] &= 0b1111'0111;
    array[7] |= static_cast<unsigned char>(center_steering_reset) << 3;
    // 15
    array[7] &= 0b1111'1011;
    array[7] |= static_cast<unsigned char>(high_voltage_switch) << 2;
    // 16
    array[7] &= 0b1111'1101;
    array[7] |= static_cast<unsigned char>(drive_motor_controller) << 1;
    // 17
    array[8] &= 0b0111'1111;
    array[8] |= static_cast<unsigned char>(highBeam) << 7;
    // 18
    array[8] &= 0b1011'1111;
    array[8] |= static_cast<unsigned char>(low_beam) << 6;
    // 19
    array[8] &= 0b1101'1111;
    array[8] |= static_cast<unsigned char>(fog_lights) << 5;
    // 20
    array[8] &= 0b1110'1111;
    array[8] |= static_cast<unsigned char>(horn) << 4;
    // 21
    array[8] &= 0b1111'0111;
    array[8] |= static_cast<unsigned char>(defense_power_on) << 3;
    // 22
    array[8] &= 0b1111'1011;
    array[8] |= static_cast<unsigned char>(fan) << 2;
    // 23
    array[8] &= 0b1111'1101;
    array[8] |= static_cast<unsigned char>(refrigeration_unit) << 1;
    // 24
    array[9] = heartbeat;
    return array;
}

void FrameDrive::DecodeFromBytes(const QByteArray &array)
{
    (void)array;
    throw std::runtime_error("FrameDrive::DecodeFromBytes not implemented");
}

QString FrameDrive::GetFrameName() const
{
    return "Drive";
}
